#include "encoder.h"

#include <stdio.h>
#include <string.h>
#include "usart.h"


int16_t encoder_l = 0, encoder_r = 0, encoder_l_last = 0, encoder_r_last = 0;

int16_t encoder_l_diff = 0;
int16_t encoder_r_diff = 0;


void encoder_init(void)
{
  TIM_SlaveConfigTypeDef sSlaveConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
	
	/*左编码器初始化*/
  htim2.Instance = TIM2;
  htim2.Init.Prescaler = 0;
  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim2.Init.Period = 65535;
  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
  {
    Error_Handler();
  }
  sSlaveConfig.SlaveMode = TIM_SLAVEMODE_EXTERNAL1;
  sSlaveConfig.InputTrigger = TIM_TS_TI1FP1;
  sSlaveConfig.TriggerPolarity = TIM_TRIGGERPOLARITY_RISING;
  sSlaveConfig.TriggerFilter = 0;
  if (HAL_TIM_SlaveConfigSynchro(&htim2, &sSlaveConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
	/*右编码器初始化*/
  htim3.Instance = TIM3;
  htim3.Init.Prescaler = 0;
  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim3.Init.Period = 65535;
  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
  {
    Error_Handler();
  }
  sSlaveConfig.SlaveMode = TIM_SLAVEMODE_EXTERNAL1;
  sSlaveConfig.InputTrigger = TIM_TS_TI1FP1;
  sSlaveConfig.TriggerPolarity = TIM_TRIGGERPOLARITY_RISING;
  sSlaveConfig.TriggerFilter = 0;
  if (HAL_TIM_SlaveConfigSynchro(&htim3, &sSlaveConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
	HAL_TIM_Base_Start_IT(&htim2);
	HAL_TIM_Base_Start_IT(&htim3);
}

// void get_encoder(void)
// {
// 	if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_SET) encoder_l = encoder_l + (__HAL_TIM_GetCounter(&htim2));
// 	else encoder_l = encoder_l - (__HAL_TIM_GetCounter(&htim2));
// 	__HAL_TIM_SetCounter(&htim2, 0);
//
// 	if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_RESET) encoder_r = encoder_r + (__HAL_TIM_GetCounter(&htim3));
// 	else encoder_r = encoder_r - (__HAL_TIM_GetCounter(&htim3));
// 	__HAL_TIM_SetCounter(&htim3, 0);
//
// }
//
// float get_speed(void)
// {
// 	float speed,linear_velocity ;
//     encoder_l_last = encoder_l;
//     encoder_r_last = encoder_r;
//     get_encoder();
//     speed = (int16_t)SPEED_GAIN * (encoder_l - encoder_l_last) +(int16_t)SPEED_GAIN * (encoder_r - encoder_r_last);
// 	linear_velocity = speed /2.0f;
// 	return linear_velocity;
// }


void get_encoder(void)
{
  int16_t cnt_l = __HAL_TIM_GetCounter(&htim2);
  int16_t cnt_r = __HAL_TIM_GetCounter(&htim3);

  // 方向判断 + 增量保存
  if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_3) == GPIO_PIN_SET)
    encoder_l_diff = cnt_l;
  else
    encoder_l_diff = -cnt_l;

  if (HAL_GPIO_ReadPin(GPIOB, GPIO_PIN_5) == GPIO_PIN_RESET)
    encoder_r_diff = cnt_r;
  else
    encoder_r_diff = -cnt_r;

  encoder_l += encoder_l_diff;
  encoder_r += encoder_r_diff;

  __HAL_TIM_SetCounter(&htim2, 0);
  __HAL_TIM_SetCounter(&htim3, 0);
}

float get_left_speed(void)
{
  return encoder_l_diff * SPEED_GAIN;
}

float get_right_speed(void)
{
  return encoder_r_diff * SPEED_GAIN;
}
